
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.io.Display;
import com.ridgesoft.robotics.AnalogInput;
import com.ridgesoft.robotics.ContinuousRotationServo;
import com.ridgesoft.robotics.Motor;
import java.io.IOException;

public class ProcessHostMessage {
    //used to check for object after heading change.
    //Needs to be remebered for two cycles
    boolean objectDetected = false;
    //Physical robot display
    private Display display;
    private byte[] message;
    private static int count = 0;
    private Mailbox hostMail;
    private Mailbox slaveMail;
    //directional information
    private char currentHeading;
    private int robotX;
    private int robotY;
    //boolean line sensor readings
    protected boolean leftOuterOverLine = false;
    protected boolean rightOuterOverLine = false;
    protected boolean leftInnerOverLine;
    protected boolean rightInnerOverLine;
    protected byte LEFT_PWR = 6;
    protected byte RIGHT_PWR = 6;
    private static final int WHITE_THRES = 150;
    private static Motor leftMotor = new ContinuousRotationServo(IntelliBrain.getServo(2), false, 14);
    private static Motor rightMotor = new ContinuousRotationServo(IntelliBrain.getServo(1), true, 14);
    protected char lastInnerOnLine;
    protected char lastOuterOnLine;
    byte leftOver = 0;
    byte rightOver = 0;
    // State definitions
    private static final byte UNK = 0; // unknown
    private static final byte S2L = 1; // 2 sensors left of line
    private static final byte S1L = 2; // 1 sensor left of line
    private static final byte CTR = 3; // centered, both sensors on line
    private static final byte S1R = 4; // 1 sensor right of line
    private static final byte S2R = 5; // 2 sensors right of line
    private static final byte LEFT = 0;
    private static final byte RIGHT = 1;

    //	State transition table. Specifies the next state given the
    //	current state and the Boolean sensor readings:
    //		0 - sensor is not over line
    //		1 - sensor is over the line
    private static byte[][] NEXT_STATE = new byte[][]{
        //			 Index (binary)
        //           00   01   10   11        Current State
        new byte[]{UNK, S1L, S1R, CTR}, // unknown
        new byte[]{S2L, S1L, S1R, CTR}, // 2 left
        new byte[]{S2L, S1L, S1R, CTR}, // 1 left
        new byte[]{UNK, S1L, S1R, CTR}, // centered
        new byte[]{S2R, S1L, S1R, CTR}, // 1 right
        new byte[]{S2R, S1L, S1R, CTR}, // 2 right
    };
    private static final int THRESHOLD = 300;

    //	Motor power table. Holds the power settings for the motors
    //	given particular states.
    private static final byte NORMAL = 6;
    private static final byte LOW = 3;
    private static final byte[][] POWER = new byte[][]{
        //           Left Right           State
        new byte[]{0, 0}, // unknown
        new byte[]{NORMAL + 1, LOW}, // 2 left
        new byte[]{NORMAL + 1, LOW}, // 1 left
        new byte[]{NORMAL, NORMAL}, // both on line
        new byte[]{LOW, NORMAL + 1}, // 1 right
        new byte[]{LOW, NORMAL + 1}, // 2 right
    };

    public ProcessHostMessage(byte[] msg, Display disp, Mailbox hMail, Mailbox sMail, char heading, int rx, int ry) throws Exception {
        if (count == 0) {
            count++;
        } else {
            throw new Exception("Wait");
        }
        this.message = msg;
        this.display = disp;
        this.hostMail = hMail;
        this.slaveMail = sMail;
        this.currentHeading = heading;
        this.robotX = rx;
        this.robotY = ry;
    }

    public void run() {
        // Determine what kind of message it is
        switch (message[0]) {
            case 1: // Navigation
                go(currentHeading, (char) message[1]);
                break;
            case 32:
                MasterBoard.heading = (char) message[1];
                MasterBoard.robotX = message[2];
                MasterBoard.robotY = message[3];
                break;
        }
        // Execute proper method to fulfil requirements of message
        count = 0;
    }
    private void go(char heading, char direction) {
     

        //Before moving, check previous state and autocorrect the robot so that
        //it is lined up with the intersection.
        try {
            switch (heading) {
                case 'N':
                    switch (direction) {
                        case 'N':
                            robotY++;
                            forwardTurn();
                            break;
                        case 'S':
                            robotY--;
                            one80Turn();
                            break;
                        case 'E':
                            robotX++;
                            rightTurn();
                            break;
                        case 'W':
                            robotX--;
                            leftTurn();
                            break;
                    }
                    break;
                case 'S':
                    switch (direction) {
                        case 'N':
                            robotY++;
                            one80Turn();
                            break;
                        case 'S':
                            robotY--;
                            forwardTurn();
                            break;
                        case 'E':
                            robotX++;
                            leftTurn();
                            break;
                        case 'W':
                            robotX--;
                            rightTurn();
                            break;
                    }
                    break;
                case 'E':
                    switch (direction) {
                        case 'N':
                            robotY++;
                            leftTurn();
                            break;
                        case 'S':
                            robotY--;
                            rightTurn();
                            break;
                        case 'E':
                            robotX++;
                            forwardTurn();
                            break;
                        case 'W':
                            robotX--;
                            one80Turn();
                            break;
                    }
                    break;
                case 'W':
                    switch (direction) {
                        case 'N':
                            robotY++;
                            rightTurn();
                            break;
                        case 'S':
                            robotY--;
                            leftTurn();
                            break;
                        case 'E':
                            robotX++;
                            one80Turn();
                            break;
                        case 'W':
                            robotX--;
                            forwardTurn();
                            break;
                    }
                    break;
            }

            //sensors read obstical but robot hasn't registered
            if(IntelliBrain.getAnalogInput(1).sample() > 400 && !objectDetected){
                //halt robot's progress and revert to previos location
                robotX = MasterBoard.robotX;
                robotY = MasterBoard.robotY;
                
                stopRobot();
                Thread.sleep(100);

                refreshOuterLineSensors();
                leftMotor.setPower(-LOW);
                rightMotor.setPower(-LOW);
              
                while(!leftOuterOverLine || !rightOuterOverLine){
                    refreshOuterLineSensors();
                }

                objectDetected = true;
                stopRobot();

                refreshOuterLineSensors();
                while(!leftOuterOverLine || !rightOuterOverLine){
                    refreshOuterLineSensors();
                    leftMotor.setPower(LOW);
                    rightMotor.setPower(LOW);
                }

                stopRobot();

                currentHeading = direction;
                MasterBoard.heading = direction;
                MasterBoard.robotX = robotX;
                MasterBoard.robotY = robotY;

                finalizeMove();
            }else{
                objectDetected = false;
                currentHeading = direction;
                MasterBoard.heading = direction;
                MasterBoard.robotX = robotX;
                MasterBoard.robotY = robotY;

               moveToIntersection();
            }
        } catch (Exception ex) {
            display.print(0, "error");
        }
     }
     private void moveToIntersection(){
         //initializes line sensors
        AnalogInput leftLineSensor = IntelliBrain.getAnalogInput(5);
        AnalogInput rightLineSensor = IntelliBrain.getAnalogInput(6);
        AnalogInput leftOuterLineSensor = IntelliBrain.getAnalogInput(4);
        AnalogInput rightOuterLineSensor = IntelliBrain.getAnalogInput(7);

        String stateString = "Starting";
        int state = UNK;
        int prevState = UNK;

        boolean go = true;
        while (go && !objectDetected) {
            int leftSample = leftLineSensor.sample();
            int rightSample = rightLineSensor.sample();

            // convert analog sensor readings to two bit index,
            // set each bit to indicate whether the associated sensor
            // sees the line
            int index = 0;
            if (leftSample > THRESHOLD) {
                index |= 0x2;
            }
            if (rightSample > THRESHOLD) {
                index |= 0x1;
            }

            prevState = state;
            // update the current state
            state = NEXT_STATE[state][index];

            LEFT_PWR = POWER[state][LEFT];
            RIGHT_PWR = POWER[state][RIGHT];
            // set the motor power according to the state
            leftMotor.setPower(POWER[state][LEFT]);
            rightMotor.setPower(POWER[state][RIGHT]);

            display.print(0, Integer.toString(leftSample) + ' ' + Integer.toString(rightSample));

            switch (state) {
                case S2L:
                    stateString = "2 Left";
                    break;
                case S1L:
                    stateString = "1 Left";
                    break;
                case CTR:
                    stateString = "Centered";
                    break;
                case S1R:
                    stateString = "1 Right";
                    break;
                case S2R:
                    stateString = "2 Right";
                    break;
                case UNK:
                    stateString = "Unknown";
                    break;
                default:
                    stateString = "Error";
                    break;
            }

            //if the robot leaves the line
            if (state == UNK) {
                //get the previous state and do it's inverse operation to find the line
                switch (prevState) {
                    case S2L:
                    case S1L:
                        leftMotor.setPower(6);
                        rightMotor.setPower(3);
                        refreshInnerLineSensors();
                        while (!rightInnerOverLine || !leftInnerOverLine) {
                            refreshInnerLineSensors();
                        }
                        break;
                    case S2R:
                    case S1R:
                        leftMotor.setPower(3);
                        rightMotor.setPower(6);
                        refreshInnerLineSensors();
                        while (!rightInnerOverLine || !leftInnerOverLine) {
                            refreshInnerLineSensors();
                        }
                        break;
                }
                LEFT_PWR = 6;
                RIGHT_PWR = 6;
                resumePower();
            }
            display.print(1, stateString);

            //checks if the robot has crossed an intersection
            refreshOuterLineSensors();
            if (leftOuterOverLine || rightOuterOverLine) {
                //give time for robot to process sensor input
                stopRobot();

                //Robot not on the Grid's edge
                if ( !(robotX == 0 || robotX == 5) && !(robotY == 0 || robotY == 5) ){
                    display.print(0, "Not on Edge");

                    //if the left outter line sensor hits the line first
                    if (leftOuterOverLine && !rightOuterOverLine) {
                        leftMotor.setPower(0);
                        //roll the right wheel forward until right line sensor hits
                        rightMotor.setPower(3);
                        while (!rightOuterOverLine) {
                             rightOuterOverLine =
                                    (IntelliBrain.getAnalogInput(7).sample() > WHITE_THRES) ? true : false;
                        }

                        go = false;
                        stopRobot();

                    //if the right outter line sensor hits the line first
                    } else if (!leftOuterOverLine && rightOuterOverLine) {
                        //roll the left wheel forward until left line sensor hits
                        leftMotor.setPower(3);
                        rightMotor.setPower(0);
                        while (!leftOuterOverLine) {
                             leftOuterOverLine =
                                     (IntelliBrain.getAnalogInput(4).sample() > WHITE_THRES) ? true : false;
                        }

                        go = false;
                        stopRobot();

                    //if both hit, stop moving
                    } else if (leftOuterOverLine && rightOuterOverLine) {
                        go = false;
                        stopRobot();
                    }
                }

                //if the Robot is on the grid edge, don't try to adjust
                else {
                    go = false;
                    stopRobot();
                }
            }
        }


        int leftOutSample = Integer.MAX_VALUE;
        int rightOutSample = Integer.MAX_VALUE;


        //Readjusts if the inner sensors are both on white
        switch (state) {
            case S2R:
                leftMotor.setPower(-6);
                while (leftOutSample < THRESHOLD) {
                    leftOutSample = leftOuterLineSensor.sample();
                }
                stopRobot();
                break;
            case S2L:
                rightMotor.setPower(6);
                leftMotor.setPower(-2);
                while (rightOutSample < THRESHOLD) {
                    rightOutSample = rightOuterLineSensor.sample();
                }

                stopRobot();
                break;
            case UNK:
                display.print(0, "DEBUG UNK");
                break;
        }

        finalizeMove();
    }

    protected static void stopRobot() {
        rightMotor.setPower(0);
        leftMotor.setPower(0);
    }
    
    //sends the robot info to host message
    private void finalizeMove() {
         display.print(0, "Out loop");
        if (slaveMail != null) {
            try {
                slaveMail.postMessage(new byte[]{0});
            } catch (IOException ex) {
            }
        }
        display.print(0, "reset count");
        try {
            hostMail.postMessage(getIntersectionInfo());
        } catch (IOException ex) {
        }
        display.print(0, "Sent intersect");
    }

    //checks for objects in front of robot                  (0 - not clear : 1 - clear)
    private byte getFrontIRsensorInfo(){
        byte fLine = (IntelliBrain.getAnalogInput(1).sample() > 400) ? (byte) 0 : (byte) 1;

        return fLine;
    }

    private byte[] getIntersectionInfo() {
        byte[] lineData = lineCheck(400);
        
        byte fLine = getFrontIRsensorInfo();
       
        try {
            Thread.sleep(3000);
        } catch (InterruptedException ex) {
        }
        byte[] info = new byte[]{(byte) currentHeading, (byte) robotX, (byte) robotY, lineData[0], fLine, lineData[1]};
        return info;
    }

    private byte[] lineCheck(int delay) {
        rightMotor.setPower(3);
        leftMotor.setPower(3);

        Move m = new Move();
        new Thread(m).start();
        try {
            Thread.sleep(delay);
        } catch (InterruptedException ex) {
        }
        m.stoprunning();

        stopRobot();

        byte[] data = new byte[]{leftOver, rightOver};
        leftOver = 0;
        rightOver = 0;
        return data;
    }

    protected void leftTurn() throws Exception {
        leftMotor.setPower(NORMAL);
        rightMotor.setPower(NORMAL);
        Thread.sleep(500);
        
        leftMotor.setPower(-NORMAL);
        rightMotor.setPower(NORMAL);
        Thread.sleep(800);


        while (!rightInnerOverLine) {
            refreshInnerLineSensors();
        }

        stopRobot();
    }

    protected void rightTurn() throws Exception {
        leftMotor.setPower(NORMAL);
        rightMotor.setPower(NORMAL);
        Thread.sleep(500);

        leftMotor.setPower(NORMAL);
        rightMotor.setPower(-NORMAL);
        Thread.sleep(800);

        while (!leftInnerOverLine) {
            refreshInnerLineSensors();
        }

        stopRobot();
    }

    protected void forwardTurn() throws Exception {
        leftMotor.setPower(NORMAL);
        rightMotor.setPower(NORMAL);
        Thread.sleep(500);
    }

    protected void one80Turn() throws Exception {
        leftMotor.setPower(-LEFT_PWR);
        rightMotor.setPower(RIGHT_PWR);
        Thread.sleep(1000);

        while (!rightInnerOverLine) {
            refreshInnerLineSensors();
        }

        stopRobot();
    }

    protected void resumePower() {
        leftMotor.setPower(LEFT_PWR);
        rightMotor.setPower(RIGHT_PWR);
    }

    /********************THE FOLLOWING SECTION OF CODE IS DEDICATED TO COLLECTING INFORMATION FROM THE ROBOTS SENSORS********************/
    protected void refreshInnerLineSensors() {
        leftInnerOverLine = (IntelliBrain.getAnalogInput(5).sample() > WHITE_THRES) ? true : false;
        rightInnerOverLine =
                (IntelliBrain.getAnalogInput(6).sample() > WHITE_THRES) ? true : false;
    }

    protected void refreshOuterLineSensors() {
        leftOuterOverLine = (IntelliBrain.getAnalogInput(4).sample() > WHITE_THRES) ? true : false;
        rightOuterOverLine =
                (IntelliBrain.getAnalogInput(7).sample() > WHITE_THRES) ? true : false;
        if (leftOuterOverLine && rightOuterOverLine) {
            lastOuterOnLine = 'B'; // Both
        }

        if (leftOuterOverLine) {
            lastOuterOnLine = 'L';
        }

        if (rightOuterOverLine) {
            lastOuterOnLine = 'R';
        }
    }

    class Move implements Runnable {

        boolean go = true;

        public void stoprunning() {
            go = false;
        }


        public void run() {
            while (go) {
                refreshOuterLineSensors();
                if (leftOuterOverLine) {
                    leftOver = 1;
                }
                if (rightOuterOverLine) {
                    rightOver = 1;
                }
            }
        }
    }
}
